#include<ros/ros.h>

#include <tf/transform_listener.h>

#include<geometry_msgs/PoseStamped.h>

#include<hri_msgs/Legs.h>

#include<visualization_msgs/MarkerArray.h>

class LegVisualizer
{
public:
    LegVisualizer();
    bool init();
    int run();

private:
    visualization_msgs::Marker createMarker(geometry_msgs::Pose leg_pose,int id, bool pair);


   void legsSubscriberCB(const hri_msgs::LegsConstPtr &leg);
   geometry_msgs::PoseStamped transform(geometry_msgs::Pose pose);

   ros::NodeHandle nh;

   ros::Subscriber legSubscriber;
   ros::Publisher leg_marke_publisher;
   tf::TransformListener *transform_listener;
};
